import queue
import logging
import threading
import time
import cv2
import numpy as np
from datetime import datetime
from algorithm.line_patrol import traget_bias_generate
from algorithm.PID import PID

class LinePatrol:
    def __init__(self):
        self.run = False
        self.__log_init()


    def __log_init(self):
        self.logger = logging.getLogger("LinePatrol")


    def open(self, 
             binary_threshold,
             search_line,
             search_width
             ):
        self.logger.trace('open.enter')
        
        self.binary_threshold = binary_threshold
        self.search_line = search_line
        self.search_width = search_width
        
        self.logger.trace('open.done')
        return True


    def close(self):
        self.logger.trace('close.enter')

        self.logger.trace('close.done')


    def thread(self):
        self.logger.trace('thread.enter')

        while self.run:
            try:
                frame, number = self.inqueue1.get(timeout=0.1)
                # self.logger.spam(f"get frame {number} from input queue")
            except queue.Empty:
                self.logger.warning("input queue is empty, waiting for next frame...")
                continue
            except Exception as e:
                self.logger.error(f"Error getting frame from input queue: {e}")
                continue
            # 转换为HSV颜色空间
            frame_RGB = cv2.cvtColor(frame, cv2.COLOR_BGR2RGB)
            hsv = cv2.cvtColor(frame_RGB, cv2.COLOR_BGR2HSV)
            
            # 创建红色掩码（两部分）
            lower_red1 = np.array([0, 100, 100])
            upper_red1 = np.array([15, 255, 255])
            lower_red2 = np.array([165, 100, 100])
            upper_red2 = np.array([179, 255, 255])
            
            mask1 = cv2.inRange(hsv, lower_red1, upper_red1)
            mask2 = cv2.inRange(hsv, lower_red2, upper_red2)
            
            # 合并两个掩码
            mask = cv2.bitwise_or(mask1, mask2)
            
            # 对原始图像和掩码进行位运算
            result = cv2.bitwise_and(frame, frame, mask=mask)
            # 灰度化
            gray_frame = cv2.cvtColor(result, cv2.COLOR_BGR2GRAY)
            # 二值化
            _, binary_frame = cv2.threshold(gray_frame, self.binary_threshold, 255, cv2.THRESH_BINARY)
            # 生成目标偏差
            target_bias = traget_bias_generate(
                binary_frame, 
                self.search_line, 
                self.search_width
            )
            # self.logger.spam(f"target_bias: {target_bias}")
            
            # 将巡线结果传递给planner
            try:
                if self.outqueue1.full():
                    self.outqueue1.get_nowait()
                self.outqueue1.put([target_bias, number], timeout=0.1)
                # self.logger.spam(f"bias: {target_bias} to output queue")
            except Exception as e:
                self.logger.error(f"Error putting pid_out to output queue: {e}")
                continue

        self.logger.trace('thread.done')


    def start(self):
        self.logger.trace('start.enter')

        self.run = True
        self.handle = threading.Thread(target=self.thread)
        self.handle.start()

        self.logger.trace('start.done')
        return True


    def stop(self):
        self.logger.trace('stop.enter')
        self.run = False
        self.handle.join()
        self.logger.trace('stop.done')


    def input_queue_set(self, input_queue_list):
        # 设置输入队列，接受的是一个队列列表
        self.logger.trace('input_queue_set.enter')

        if self.run:
            self.logger.error("LinePatrol is opened, please close the this first!")
            return False
        
        if not isinstance(input_queue_list, list):
            input_queue_list = [input_queue_list]

        for input_queue in input_queue_list:
            if not isinstance(input_queue, queue.Queue):
                self.logger.error("one queue is not a queue!")
                return False

        self.inqueue1 = input_queue_list[0] 

        self.logger.trace('input_queue_set.done')
        return True


    def output_queue_set(self, output_queue_list):
        # 设置输出队列，接受的是一个队列列表
        self.logger.trace('output_queue_set.enter')

        if self.run:
            self.logger.error("LinePatrol is opened, please close the this first!")
            return False
        
        if not isinstance(output_queue_list, list):
            output_queue_list = [output_queue_list]

        for output_queue in output_queue_list:
            if not isinstance(output_queue, queue.Queue):
                self.logger.error("one queue is not a queue!")
                return False

        self.outqueue1 = output_queue_list[0] 

        self.logger.trace('output_queue_set.done')
        return True